spauvfandomcom-20200214-history
Sp auv colortest.cpp
/* Singapore Polytechnic Singapore Autonomous Underwater Vehicle Challenge 2014 Computer Vision System Version : PRE ALPHA */ //Include Libraries #include #include #include #include #include #include #include #include #include "ros/ros.h" #include "std_msgs/String.h" #include "std_msgs/Int16.h" #include "geometry_msgs/Point32.h" #include #include //Declarations using namespace cv; int state = 0; int hitflare = 0; int count = 0; bool ifgui = true; Mat hsvbottom; int H; int S; int V; int bot_L_r; int bot_L_g; int bot_L_b; int bot_U_r; int bot_U_g; int bot_U_b; int main(int argc, char **argv) { //SETUP ros::init(argc, argv, "auv_camera"); ros::NodeHandle n; ros::Publisher camera_pub_bl = n.advertise("cam_black", 1000); ros::Publisher camera_pub_rd = n.advertise("cam_red", 1000); ros::Publisher camera_pub_yl = n.advertise("cam_yellow", 1000); ros::Publisher camera_pub_state = n.advertise("cam_state", 1000); ros::Rate loop_rate(10); std_msgs::String cam_state; geometry_msgs::Point32 cam_black; geometry_msgs::Point32 cam_red; geometry_msgs::Point32 cam_blue; //Initialise Bottom Camera FILE * pfile; pfile = fopen("results.txt","a"); fprintf(pfile," \n"); fprintf(pfile,"Start of test:\n"); VideoCapture cambottom(1); if(!cambottom.isOpened()) //Checking sequence { std::cout<<"\n Not found \n"; return -1; } cambottom.set(CV_CAP_PROP_FRAME_WIDTH, 320); cambottom.set(CV_CAP_PROP_FRAME_HEIGHT, 240); namedWindow("Control", CV_WINDOW_AUTOSIZE); int iLowH = 0; int iHighH = 179; int iLowS = 0; int iHighS = 255; int iLowV = 0; int iHighV = 255; cvCreateTrackbar("LowH", "Control", &iLowH, 179); cvCreateTrackbar("HighH", "Control", &iHighH, 179); cvCreateTrackbar("LowS", "Control", &iLowS, 255); cvCreateTrackbar("HighS", "Control", &iHighS, 255); cvCreateTrackbar("LowV", "Control", &iLowV, 255); cvCreateTrackbar("HighV", "Control", &iHighV, 255); if(ifgui) namedWindow("Original Frame (Bottom)",1); if(ifgui) namedWindow("Thresholded Frame (Bottom)",1); cam_state.data="black"; camera_pub_state.publish(cam_state); while(1) { count += 1; //pfile = fopen("results.txt","w"); Mat framebottom; cambottom >> framebottom; // Get a new frame from camera GaussianBlur(framebottom, framebottom, Size(7,7), 1.5, 1.5); cvtColor(framebottom, hsvbottom, CV_BGR2HSV); Mat imgThresholded; inRange(framebottom, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); erode(imgThresholded, imgThresholded,getStructuringElement(MORPH_ELLIPSE, Size(5,5))); //Eliminate the small particles dilate(imgThresholded, imgThresholded,getStructuringElement(MORPH_ELLIPSE, Size(5,5))); erode(imgThresholded, imgThresholded,getStructuringElement(MORPH_ELLIPSE, Size(5,5))); dilate(imgThresholded, imgThresholded,getStructuringElement(MORPH_ELLIPSE, Size(5,5))); Moments oMoments = moments(imgThresholded); double dM01 = oMoments.m01; double dM10 = oMoments.m10; double dArea = oMoments.m00; if(dArea >2000) { int posX = dM10/dArea; int posY = dM01/dArea; if(ifgui) circle(framebottom,cvPoint(posX,posY),5,cvScalar(0,255,255),4); } //unsigned char*input = (unsigned char*)(hsvbottom.data); unsigned char*input = (unsigned char*)(framebottom.data); double totalH = 0; double totalS = 0; double totalV =0; double count = 0; //for(int i = 110; i < 130; i++){ //rows //for(int j= 150; j< 170;j++){ //columns //input* i + j*3 = 255; //input* i + j*3 + 1 = 255; //input* i + j*3 + 2 = 255; //} // } for(int i = 110; i < 130; i++){ //rows for(int j= 150; j< 170;j++){ //columns //H = input* i + j*3 ; //Hue H= input* i + j*3 ; //blue totalH = totalH + H; //S = input* i + j*3 + 1 ; //saturation S= input* i + j*3 + 1 ; //green totalS = totalS + S; //V = input* i + j*3 + 2 ; //value V = input* i + j*3 + 2 ; //red totalV = totalV + V; count++; } } totalH = totalH / count; totalS = totalS / count; totalV = totalV / count; printf (" H %d S %d V %d \n",(unsigned char) totalH,(unsigned char) totalS,(unsigned char) totalV); CvPoint P1; P1.x=150; P1.y=110; CvPoint P2; P2.x=170; P2.y=130; rectangle(framebottom, P1 , P2, Scalar(0,0,255),1,8); if(ifgui) imshow("Original Frame (Bottom)", framebottom); if(ifgui) imshow("Black Thresholded Frame (Bottom)", imgThresholded); if(waitKey(30) >= 0) break; } // the camera will be deinitialized automatically in VideoCapture destructor return 0; }